﻿using System;
using COC.Tools.Pathing.AIViewModel;
using COC.Tools.Pathing.Misc;
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using COC.Threading;
namespace COC.Tools.Pathing
{
    /// <summary>
    /// a* algorithm method
    /// </summary>
    public sealed class PathfindingMath
    {


        DateTime startTime;
        TimeSpan consumedTime;
        #region tileParameters
        public float mTileSize { get; set; }
        public float mOffset { get; set; }

        public CTile[] mBHVArray { get; set; }
        public Transform mAnsestor { get; set; }
        public static int mRows { get; set; }
        public static int mColumns { get; set; }
        public float mHeight { get; set; }
        public GridNode[,] gridNodes = null;
        #endregion
        bool s;

        #region BinaryHeap

        Dictionary<int, CBinaryHeap> binaryHeaps = new Dictionary<int, CBinaryHeap>();

        #endregion

        #region properties

        static PathfindingMath aStartMath = new PathfindingMath();
        List<CTile> openList = new List<CTile>();
        Dictionary<int, PathNode> followersGraphNodeInfo = new Dictionary<int, PathNode>();//save each follower's owned graph nodes informations into list
        public Dictionary<int, PathInfo> followersPathInfo = new Dictionary<int, PathInfo>();//save each follower's owned path informations into list

        public Dictionary<int, CPath> followersPath = new Dictionary<int, CPath>();//save each follower's owned path informations into list



        public byte walkable = 0x0;
        public byte unWalkable = 0x1;
        byte unVisited = 0x02;
        byte visited = 0x3;
        byte onOpenList = 0x0;
        public bool stopped = false;
        Dictionary<int, int> storedPathFinderPathLength = new Dictionary<int, int>();
        int pathStatus = 0;
        #endregion
        #region a* pathfinding internal field

        Dictionary<int, byte[,]> wishList = new Dictionary<int, byte[,]>();//key=chase value which point have already  accessed.

        public static CTile[,] mObstacleMap;
        private byte[,] tempWitchList;
        private int tempFollowerPathLength = 0;

        #endregion

        private CThreadPool threadPool;
        private CEventQueue doPathQueue;
        private CEventQueue postResultQueue;
        public static PathfindingMath getInstance()
        {
            return aStartMath;
        }
        public void Initial()
        {
            stepGameObject = new GameObject();
            stepGameObject.name = "Steps";
        }

        public void configuration(float[] gridDem, CTile[,] obstacleMap)
        {

            mRows = (int)gridDem[0];
            mColumns = (int)gridDem[1];
            mTileSize = gridDem[2];
            mOffset = gridDem[3];
            mObstacleMap = obstacleMap;
        }
        public void start()
        {

            CThreadPool.CThreadPoolInfo tpOption = new CThreadPool.CThreadPoolInfo();
            tpOption.isBackgound = true;
            tpOption.mPriority = CThreadPool.CThread.CThreadOption.Policy.DEFAULT;
            doPathQueue = new CEventQueue();
            postResultQueue = new CEventQueue();
            threadPool = new CThreadPool(PathFindingConstant.PATH_THREAD_POOL, 4, doPathQueue, postResultQueue, null, tpOption);
            threadPool.Start();

        }





        #region a star pathing function
        public IEnumerator FindPath(CPath p)
        {
            CPath currPath = getPathData(p);
            int[,] currWhichList = currPath.whichList;
            CBinaryHeap currBinaryHeap = adjustBinaryHeap(currPath.cBinaryHeap);
            resetPathInfo(currPath.mPathInfo);
            // PathInfo pathInfo = currPath.mPathInfo;
            UnityEngine.Debug.Log("[FOLLOWER ID]" + currPath.id + " calculating...");
            //            string s = "";//print unvisited node on screen. 
            int startRow;
            int startCol;
            int targetRow;
            int targetCol;
            float startingX;
            float startingY;
            float destX;
            float destY;
            computingCoordinate(currPath.id, currPath.startingXY, currPath.targetXY, out startRow, out startCol, out targetRow, out targetCol, out startingX, out startingY, out destX, out destY);

            int a, b = 0;

            //            Vector3 startNewPos = new Vector3(currPath.targetXY.x, currPath.targetXY.y, currPath.targetXY.z);
            //            var time = 1.0f;
            //            int path = 0;

            currBinaryHeap.clear();
            if (targetRow >= mRows || targetCol >= mColumns)
            {
                UnityEngine.Debug.Log("could not reach it.");
                yield break;
            }
            //	If starting location and target are in the same location...
            if (Math.Abs(startingX - destX) < 1 && Math.Abs(startingY - destY) < 1)
            {
                UnityEngine.Debug.Log("could not access.---CASE ONE");
                currPath.STATE = PathInfo.PathState.ONE;
                yield break;
            }
            if (mObstacleMap[targetRow, targetCol].walkable == GraphNode.State.UNWALKABLE)
            {
                //TODO
                UnityEngine.Debug.Log("could not access.----CASE TWO");
                //notify to inform this target locaiton is not walkable. 
                currPath.STATE = PathInfo.PathState.NONE;
                yield break;
            }
            if (Math.Abs(startingX - destX) > 1 || Math.Abs(startingY - destY) > 1)
            {

                PathNode pathNode = null;






                int parentX = startRow;
                int parentY = startCol;
                int corner = 0x0;
                int tempGcost = 0;
                int pathX = 0;
                int pathY = 0;
                int tempX = 0;
                currPath.node[startRow, startCol] = new GraphNode();
                currPath.node[startRow, startCol].gcost = 0;
                currPath.node[startRow, startCol].index = 1;
                GraphNode.State.VISITED = GraphNode.State.VISITED + 2;
                GraphNode.State.UNVISITED = GraphNode.State.VISITED - 1;
                PathNode mHeapNodes = new PathNode();
                mHeapNodes.x = startRow;
                mHeapNodes.y = startCol;
                currBinaryHeap.addNode(mHeapNodes);
                //  int[] tempXY = new int[2];
                currWhichList[startRow, startCol] = GraphNode.State.VISITED;//first,check the first node( ID#1 ) have already accessed.
                startTime = System.DateTime.Now;

                //openList.Add(1,1);
                //	Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
                do
                {


                    //If the open list is not empty, take the first cell off of the list.
                    //	This is the lowest F cost cell on the open list.
                    if (currBinaryHeap.numberOfOpenList != 0)
                    {
                        UnityEngine.Debug.Log("[CURRENT NUMBER:]" + currBinaryHeap.numberOfOpenList);
                        parentX = currBinaryHeap.mBinaryHeap[1].x;
                        parentY = currBinaryHeap.mBinaryHeap[1].y;
                        currWhichList[parentX, parentY] = GraphNode.State.VISITED;

                        // currBinaryHeap.numberOfOpenList = currBinaryHeap.numberOfOpenList - 1; ///reduce number of open list items by 1	
                        //	Delete the top item in binary heap and reorder the heap, with the lowest F cost item rising to the top.
                        //move the last item in the heap up to slot #1


                        //	Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
                        // Pop the first item off the open list.
                        #region Remove node in heap
                        currBinaryHeap.removeNode();
                        #endregion
                        int addedGCost = 0;
                        for (a = parentX - 1; a <= parentX + 1; a++)
                        {
                            for (b = parentY - 1; b <= parentY + 1; b++)
                            {
                                //1.we need a valid tile
                                if (a != -1 && b != -1 && a != mRows && b != mColumns)
                                {

                                    //2.we need to put a unvisited position xy or a none-opened position into binary heap.
                                    if (currWhichList[a, b] != GraphNode.State.VISITED)
                                    {
                                        //	Don't cut across corners
                                        //3.if not obstacles
                                        if (mObstacleMap[a, b].walkable != GraphNode.State.UNWALKABLE)
                                        {
                                            //	Don't cut across corners
                                            corner = GraphNode.State.WALKABLE;
                                            if (a == parentX - 1)
                                            {
                                                if (b == parentY - 1)
                                                {
                                                    if (mObstacleMap[parentX - 1, parentY].walkable == GraphNode.State.UNWALKABLE
                                                        || mObstacleMap[parentX, parentY - 1].walkable == GraphNode.State.UNWALKABLE)
                                                        corner = GraphNode.State.UNWALKABLE;
                                                }
                                                else if (b == parentY + 1)
                                                {
                                                    if (mObstacleMap[parentX, parentY + 1].walkable == GraphNode.State.UNWALKABLE
                                                        || mObstacleMap[parentX - 1, parentY].walkable == GraphNode.State.UNWALKABLE)
                                                        corner = GraphNode.State.UNWALKABLE;
                                                }
                                            }
                                            else if (a == parentX + 1)
                                            {
                                                if (b == parentY - 1)
                                                {
                                                    if (mObstacleMap[parentX, parentY - 1].walkable == GraphNode.State.UNWALKABLE
                                                        || mObstacleMap[parentX + 1, parentY].walkable == GraphNode.State.UNWALKABLE)
                                                        corner = GraphNode.State.UNWALKABLE;
                                                }
                                                else if (b == parentY + 1)
                                                {
                                                    if (mObstacleMap[parentX + 1, parentY].walkable == GraphNode.State.UNWALKABLE
                                                        || mObstacleMap[parentX, parentY + 1].walkable == GraphNode.State.UNWALKABLE)
                                                        corner = GraphNode.State.UNWALKABLE;
                                                }
                                            }
                                            if (corner != GraphNode.State.UNWALKABLE)
                                            {


                                                //4.Create a new open list item in the binary heap.
                                                if (currWhichList[a, b] != GraphNode.State.UNVISITED)
                                                {

                                                    pathNode = new PathNode();

                                                    pathNode.x = a;//record coordinate x y for new item 
                                                    pathNode.y = b;


                                                    if (Mathf.Abs(a - parentX) == 1 && Mathf.Abs(b - parentY) == 1)
                                                    {
                                                        addedGCost = 14;
                                                    }
                                                    else
                                                    {
                                                        addedGCost = 10;
                                                    }
                                                    currPath.node[a, b] = new GraphNode();
                                                    currPath.node[a, b].gcost = (currPath.node[parentX, parentY].gcost + addedGCost);
                                                    currPath.node[a, b].parentNode.x = parentX;
                                                    currPath.node[a, b].parentNode.y = parentY;
                                                    pathNode.hCost = 10 * (Mathf.Abs(a - targetRow) + Mathf.Abs(b - targetCol));

                                                    pathNode.fCost = pathNode.hCost + currPath.node[a, b].gcost;
                                                    //2.Move the new open list item to the proper place in the binary heap.
                                                    //Starting at the bottom, successively compare to parent items,
                                                    //swapping as needed until the item finds its place in the heap
                                                    //or bubbles all the way to the top (if it has the lowest F cost). 
                                                    #region add node into heap.

                                                    currBinaryHeap.addNode(pathNode);
                                                    #endregion

                                                    //Change whichList to show that the new item is on the open list.
                                                    currWhichList[a, b] = GraphNode.State.UNVISITED;

                                                    // Debug.Log("follower[" + followerId + "] whichList[" + a + "," + b + "] = " + (s = (whichList[a, b] == unVisited) ? "unVisited" : ""));
                                                }
                                            }
                                        }

                                    }
                                    else
                                    {
                                        ////8.If adjacent cell is already on the open list, check to see if this 
                                        //	path to that cell from the starting location is a better one. 
                                        //	If so, change the parent of the cell and its G and F costs.	
                                        //Figure out the G cost of this possible new path
                                        if (Mathf.Abs(a - parentX) == 1 && Math.Abs(b - parentY) == 1)
                                            addedGCost = 14;//cost of going to diagonal tiles	
                                        else
                                            addedGCost = 10;//cost of going to non-diagonal tiles				
                                        tempGcost = currPath.node[parentX, parentY].gcost + addedGCost;
                                        //If this path is shorter (G cost is lower) then change
                                        //the parent cell, G cost and F cost. 	
                                        if (tempGcost < currPath.node[a, b].gcost)
                                        {
                                            currPath.node[a, b].parentNode.x = parentX;
                                            currPath.node[a, b].parentNode.y = parentY;
                                            currPath.node[a, b].gcost = tempGcost;

                                            for (int i = 1; i < currBinaryHeap.numberOfOpenList; i++)
                                            {
                                                if (currBinaryHeap.mBinaryHeap[i].x == a && currBinaryHeap.mBinaryHeap[i].y == b)
                                                {
                                                    currBinaryHeap.mBinaryHeap[i].fCost = currPath.node[a, b].gcost + currBinaryHeap.mBinaryHeap[i].hCost;
                                                }
                                            }

                                            #region reorder the heap,and lowest f cost on the top of this heap.
                                            currBinaryHeap.rearrange();
                                            #endregion

                                        }

                                    }



                                }
                            }
                        }
                        if (a == targetRow && b == targetCol)
                        {
                            UnityEngine.Debug.Log("whether same: YES" + ((currWhichList[targetRow, targetCol] == GraphNode.State.UNVISITED) ? "true" : "false"));
                        }

                    }
                    else
                    {
                        currPath.STATE = PathInfo.PathState.NONE;
                        consumedTime = System.DateTime.Now - startTime;
                        UnityEngine.Debug.Log("follower[" + currPath.id + "] targetRow=" + targetRow + ",targetCol=" + targetCol + "have not found.consumed time:" + String.Format("{0}.{1}", consumedTime.Seconds, consumedTime.Milliseconds.ToString().PadLeft(3, '0')));
                        break;
                    }


                    //if exist a target position x z in whichList,then found it
                    if (currWhichList[targetRow, targetCol] == GraphNode.State.UNVISITED)
                    {
                        UnityEngine.Debug.Log("follower[" + currPath.id + "] targetRow=" + targetRow + ",targetCol=" + targetCol + "have found");
                        currPath.STATE = PathInfo.PathState.ONE;
                        break;
                    }

                } while (!stopped);
                if (currPath.STATE == PathInfo.PathState.ONE)
                {
                    //a.Working backwards from the target to the starting location by checking
                    //	each cell's parent, figure out the length of the path.
                    consumedTime = System.DateTime.Now - startTime;
                    UnityEngine.Debug.Log("[FOLLOWER ID] :" + currPath.id + "I'm found target." + "[" + currPath.targetXY.ToString() + "] position,and consumed time:" + String.Format("{0}.{1}", consumedTime.Seconds, consumedTime.Milliseconds.ToString().PadLeft(3, '0')));
                    // pathX = targetRow; pathY = targetCol;
                    /* if (!storedPathFinderPathLength.TryGetValue(followerId, out tempFollowerPathLength))
                         storedPathFinderPathLength.Add(followerId, 0);
 */

                    /*      do
                          {
                              tempX = currPath.node[pathX, pathY].parentNode.x;
                              pathY = currPath.node[pathX, pathY].parentNode.y;
                              pathX = tempX;
                              //Figure out the path length
                              currPath.mPathInfo.length = currPath.mPathInfo.length + 1;
                          } while (pathX != startRow || pathY != startCol);*/
                    //b.Now copy the path information over to the databank. Since we are
                    //	working backwards from the target to the start location, we copy
                    //	the information to the data bank in reverse order. The result is
                    //	a properly ordered set of path data, from the first step to the
                    //	last.
                    pathX = targetRow; pathY = targetCol;
                    currPath.mPathInfo.pathBank.Clear();
                    do
                    {
                        //cellPositionX = cellPositionX - 2;//work backwards 2 integers
                        // Debug.Log("------------------------->>>>>cellPositionX :pathX,pathY[" + pathX+","+pathY+"]  <<<<----------------------------");
                        currPath.mPathInfo.pathBank.Add(new float[] { pathX, pathY });


                        //c.Look up the parent of the current cell.	
                        tempX = currPath.node[pathX, pathY].parentNode.x;
                        pathY = currPath.node[pathX, pathY].parentNode.y;
                        pathX = tempX;
                        currPath.mPathInfo.length = currPath.mPathInfo.length + 1;
                        //d.If we have reached the starting square, exit the loop.	
                    }

                    while (pathX != startRow || pathY != startCol);
                    //-----------------------DEBUG-----------------------------
                    //draw line on screen if enable debug 
                    if (false)
                    {
                        float[] curr = null;
                        float[] next = null;
                        for (int d = 0; d < currPath.mPathInfo.pathBank.Count - 1; d++)
                        {

                            curr = currPath.mPathInfo.pathBank[d];
                            next = currPath.mPathInfo.pathBank[d + 1];
                            UnityEngine.Debug.DrawLine(new Vector3(curr[0] + (mTileSize + mOffset) + (mTileSize / 2), 1.5f, curr[1] + (mTileSize + mOffset) + (mTileSize / 2)), new Vector3(next[0] + (mTileSize + mOffset) + (mTileSize / 2), 1.5f, next[1] + (mTileSize + mOffset) + (mTileSize / 2)), Color.green);

                        }

                    }
                    //e.mark this shorter path for follower

                    UnityEngine.Debug.Log("[FOLLOWER ID]:" + currPath + " starting to move. Total step is " + currPath.mPathInfo.pathBank.Count);
                    ReadPath2(currPath.id, currPath.startingXY.x, currPath.startingXY.z, currPath.startingXY.y);

                }
                UnityEngine.Debug.Log("[[[[[[[[[[NEXT PATH]]]]]]]]]]]");
                yield return null;
            }


        }

        private void computingCoordinate(int followerId, Vector3 start, Vector3 target, out int rStartRow, out int rStartCol, out int targetRow,
            out int targetCol, out float startingX, out float startingY, out float rDestX, out float rDestY)
        {
            rStartRow = ((int)start.x / (int)(mTileSize + mOffset));
            rStartCol = ((int)start.z / (int)(mTileSize + mOffset));
            targetRow = ((int)target.x / (int)(mTileSize + mOffset));
            targetCol = ((int)target.z / (int)(mTileSize + mOffset));
            // var GraphNodeHandler = initializeRequiredParameters(followerId, out pathInfo);
            startingX = Mathf.Round(((rStartRow == 0 ? (mTileSize + mOffset) : (rStartRow * (mTileSize + mOffset)))));
            startingY = Mathf.Round(((rStartCol == 0 ? (mTileSize + mOffset) : (rStartCol * (mTileSize + mOffset)))));
            rDestX = Mathf.Round(((targetRow == 0 ? (mTileSize + mOffset) : (targetRow * (mTileSize + mOffset)))));
            rDestY = Mathf.Round(((targetCol == 0 ? (mTileSize + mOffset) : (targetCol * (mTileSize + mOffset)))));
            /* for (int i = 0; i < gridNodes.GetLength(0);i++ )
             {
                 for (int j = 0; j < gridNodes.GetLength(1); j++)
                 {
                     if (gridNodes[i, j].x == rStartRow && gridNodes[i, j].y == rStartCol)
                     {
                         rStartRow = i;
                         rStartCol = j;
                         break;
                     }
                 }
             }
             for (int i = 0; i < gridNodes.GetLength(0); i++)
             {
                 for (int j = 0; j < gridNodes.GetLength(1); j++)
                 {
                     if (gridNodes[i, j].x == rDestX && gridNodes[i, j].y == rDestY)
                     {
                         rDestX = i;
                         rDestY = j;
                         break;
                     }
                 }
             }*/
            UnityEngine.Debug.Log("FOLLOWER ID :" + followerId + "[FOLLOWER ORIGIN POSITION]: [" + rStartRow + "," + rStartCol + "] " +
                      "[TARGET POSITION]: [" + targetRow + "," + targetCol + "].");
        }

        private CBinaryHeap adjustBinaryHeap(CBinaryHeap cBinaryHeap)
        {
            CBinaryHeap heap = cBinaryHeap;
            return heap;
        }

        private void resetPathInfo(PathInfo pathInfo)
        {
            pathInfo.STATE = PathInfo.PathState.NONE;

        }

        private CPath getPathData(CPath path)
        {

            if (!followersPath.ContainsKey(path.id))
            {
                CBinaryHeap binaryHeap = new CBinaryHeap((int)mRows, (int)mColumns);
                PathInfo pathInfo = new PathInfo();
                path.construction((int)mRows, (int)mColumns);
                path.cBinaryHeap = binaryHeap;
                path.mPathInfo = pathInfo;
                followersPath.Add(path.id, path);
            }
            return followersPath[path.id];
        }
        private CBinaryHeap findBinaryHeapByFollower(int followerId)
        {
            CBinaryHeap heap = null;
            if (binaryHeaps.ContainsKey(followerId))
            {
                if (binaryHeaps.TryGetValue(followerId, out heap))
                {
                    return heap;
                }
                binaryHeaps.Remove(followerId);
            }
            heap = new CBinaryHeap((int)mRows, (int)mColumns);
            binaryHeaps.Add(followerId, heap);
            return heap;

        }
        [Obsolete("Instead of FindPath(int followerId, Vector3 start, Vector3 target)")]
        public IEnumerator FindPath(int followerId, Transform start, Transform target)
        {
            UnityEngine.Debug.Log("[FOLLOWER ID]" + followerId + " calculating...");
            //            string s = "";//print unvisited node on screen. 
            int startRow = (int)(start.localPosition.x / (mTileSize + mOffset)) + 1;
            int startCol = (int)(start.localPosition.z / (mTileSize + mOffset)) + 1;
            int targetRow = (int)(target.localPosition.x / (mTileSize + mOffset)) + 1;
            int targetCol = (int)(target.localPosition.z / (mTileSize + mOffset)) + 1;
            PathInfo pathInfo;
            var graphNodeInfo = initializeRequiredParameters(followerId, out pathInfo);
            float startingX = Mathf.Round((startRow * (mTileSize + mOffset)));
            float startingY = Mathf.Round((startCol * (mTileSize + mOffset)));
            int a = 0;
            int b = 0;
            Vector3 startNewPos = new Vector3(target.localPosition.x, target.localPosition.y, target.localPosition.z);
            UnityEngine.Debug.Log("FOLLOWER ID :" + followerId + "[FOLLOWER ORIGIN POSITION]: [" + start.localPosition.x + "," + start.localPosition.y + "," + start.localPosition.z + "] " + "[TARGET POSITION]: [" + startNewPos.x + "," + startNewPos.y + "," + startNewPos.z + "].");
            //            var time = 1.0f;
            //            int path = 0;
            //            float speed = 1.0f / time;
            // while (start.position != startNewPos)
            // {
            //    start.localPosition = Vector3.MoveTowards(start.localPosition, startNewPos, speed);
            //   yield return null;
            // }
            float destX = Mathf.Round((targetRow * (mTileSize + mOffset)));
            float destY = Mathf.Round((targetCol * (mTileSize + mOffset)));
            graphNodeInfo.numberOfOpenListItems = 1;
            /*
                    for (int i = 0; i<mBHVArray.Length; i++)
                    {
                        if(Math.Abs(mBHVArray[i].mOrigin.x - startingX) < 1&&Math.Abs(mBHVArray[i].mOrigin.z - startingY) < 1)
                        {
                            UnityEngine.Debug.Log("index at:=" + mBHVArray[i].index+ " has a vector [" + mBHVArray[i].mOrigin.x+","+mBHVArray[i].mOrigin.z+"] which location ["+mBHVArray[i].pointX+","+mBHVArray[i].pointY+"]contained of "+"["+start.localPosition.x+","+start.localPosition.z+"].");
                        }
                        yield return null;
                    }
                   */
            //	If starting location and target are in the same location...
            if (Math.Abs(startingX - destX) < 1 && Math.Abs(startingY - destY) < 1 && followersPathInfo[followerId].location > 0)
            {

                pathInfo.STATE = PathInfo.PathState.ONE;
            }
            if (Math.Abs(startingX - destX) < 1 && Math.Abs(startingY - destY) < 1 && followersPathInfo[followerId].location == 0)
            {

                pathInfo.STATE = PathInfo.PathState.NONE;
            }
            if (mObstacleMap[targetRow - 1, targetCol - 1].walkable == unWalkable)
            {
                //TODO
                //notify to inform this target locaiton is not walkable. 
                pathInfo.STATE = PathInfo.PathState.NONE;

                yield break;
            }
            if (Math.Abs(startingX - destX) > 1 || Math.Abs(startingY - destY) > 1)
            {

                byte[,] whichList = new byte[(int)mRows, (int)mColumns]; // dimensional array used to record 
                // 		whether a cell is on the open list or on the closed list.
                graphNodeInfo.mGCost = new int[(int)mRows, (int)mColumns];
                graphNodeInfo.mHCost = new int[(int)mRows * (int)mColumns + 2];
                graphNodeInfo.mFCost = new int[(int)mRows * (int)mColumns + 2];
                graphNodeInfo.MParentNode = new PathNode.ParentNode[(int)mRows, (int)mColumns + 2];

                if (!wishList.TryGetValue(followerId, out tempWitchList))
                    wishList.Add(followerId, whichList);

                Dictionary<int, int[]> openList = new Dictionary<int, int[]>(); //1 dimensional array holding ID# of open list items
                //                int u, v;
                //                u = v = 1;
                int parentX = startRow;
                int parentY = startCol;
                int m = 1;
                int newOpenListItemID = 0;
                int temp = 0;
                byte corner = 0x0;
                int tempGcost = 0;
                int pathX = 0;
                int pathY = 0;
                int tempX = 0;
                graphNodeInfo.mGCost[parentX, parentY] = 0;
                int[] tempXY = new int[2];
                whichList[startRow, startCol] = visited;//first,check the first node( ID#1 ) have already accessed.
                startTime = System.DateTime.Now;

                do
                {


                    //print("follower["+followerId+"]  mGraphNodeInfo.numberOfOpenListItems =" + GraphNodeHandler.numberOfOpenListItems);
                    //If the open list is not empty, take the first cell off of the list.
                    //	This is the lowest F cost cell on the open list.
                    if (graphNodeInfo.numberOfOpenListItems != 0)
                    {
                        // Pop the first item off the open list.
                        whichList[parentX, parentY] = visited;
                        graphNodeInfo.numberOfOpenListItems = graphNodeInfo.numberOfOpenListItems - 1; ///reduce number of open list items by 1	
                        //	Delete the top item in binary heap and reorder the heap, with the lowest F cost item rising to the top.
                        openList.TryGetValue(graphNodeInfo.numberOfOpenListItems + 1, out tempXY); ;//move the last item in the heap up to slot #1
                        if (tempXY != null)
                        {
                            parentX = tempXY[0];
                            parentY = tempXY[1];
                            // Debug.Log("follower["+followerId+"] parentX = " + tempXY[0] + ";" + "parentY = " + tempXY[1]);

                        }

                        /*  if (u * 2 + 1 <= numberOfOpenListItems)//if exsit both child
                          {
                       
                          }*/


                        int addedGCost = 0;
                        for (a = parentX - 1; a <= parentX + 1; a++)
                        {
                            for (b = parentY - 1; b <= parentY + 1; b++)
                            {
                                //1.we need a valid tile
                                if (a != -1 && b != -1 && a != mRows && b != mColumns)
                                {
                                    //2.we need to put a unvisited position xy or a none-opened position into binary heap.
                                    if (whichList[a, b] != visited)
                                    {
                                        //3.if not obstacles
                                        if (mObstacleMap[a, b].walkable != unWalkable)
                                        {
                                            //	Don't cut across corners
                                            corner = walkable;
                                            if (a == parentX - 1)
                                            {
                                                if (b == parentY - 1)
                                                {
                                                    if (mObstacleMap[parentX - 1, parentY].walkable == unWalkable
                                                        || mObstacleMap[parentX, parentY - 1].walkable == unWalkable)
                                                        corner = unWalkable;
                                                }
                                                else if (b == parentY + 1)
                                                {
                                                    if (mObstacleMap[parentX, parentY + 1].walkable == unWalkable
                                                        || mObstacleMap[parentX - 1, parentY].walkable == unWalkable)
                                                        corner = unWalkable;
                                                }
                                            }
                                            else if (a == parentX + 1)
                                            {
                                                if (b == parentY - 1)
                                                {
                                                    if (mObstacleMap[parentX, parentY - 1].walkable == unWalkable
                                                        || mObstacleMap[parentX + 1, parentY].walkable == unWalkable)
                                                        corner = unWalkable;
                                                }
                                                else if (b == parentY + 1)
                                                {
                                                    if (mObstacleMap[parentX + 1, parentY].walkable == unWalkable
                                                        || mObstacleMap[parentX, parentY + 1].walkable == unWalkable)
                                                        corner = unWalkable;
                                                }
                                            }
                                            if (corner != unWalkable)
                                            {
                                                //4.Create a new open list item in the binary heap.
                                                if (whichList[a, b] != unVisited)
                                                {
                                                    newOpenListItemID = newOpenListItemID + 1; //each new item has a unique ID #
                                                    m = graphNodeInfo.numberOfOpenListItems + 1;

                                                    openList[m] = new int[] { a, b };//record coordinate x y for new item 
                                                    if (Mathf.Abs(a - parentX) == 1 && Mathf.Abs(b - parentY) == 1)
                                                    {
                                                        addedGCost = 14;
                                                    }
                                                    else
                                                    {
                                                        addedGCost = 10;
                                                    }

                                                    graphNodeInfo.mGCost[a, b] = (graphNodeInfo.mGCost[parentX, parentY] + addedGCost);
                                                    graphNodeInfo.MParentNode[a, b] = new PathNode.ParentNode();
                                                    graphNodeInfo.mHCost[m] = 10 * (Mathf.Abs(a - targetRow - 1) + Mathf.Abs(b - targetCol - 1));
                                                    graphNodeInfo.MParentNode[a, b].x = parentX;
                                                    graphNodeInfo.MParentNode[a, b].y = parentY;
                                                    graphNodeInfo.mFCost[m] = graphNodeInfo.mHCost[m] + graphNodeInfo.mGCost[a, b];
                                                    //2.Move the new open list item to the proper place in the binary heap.
                                                    //Starting at the bottom, successively compare to parent items,
                                                    //swapping as needed until the item finds its place in the heap
                                                    //or bubbles all the way to the top (if it has the lowest F cost). 
                                                    while (m != 1)
                                                    {

                                                        if (graphNodeInfo.mFCost[m] < graphNodeInfo.mFCost[m / 2])
                                                        {
                                                            temp = graphNodeInfo.mFCost[m / 2];
                                                            graphNodeInfo.mFCost[m / 2] = graphNodeInfo.mFCost[m];
                                                            graphNodeInfo.mFCost[m] = temp;
                                                            m /= 2;
                                                        }
                                                        else
                                                            break;

                                                    }
                                                    graphNodeInfo.numberOfOpenListItems = graphNodeInfo.numberOfOpenListItems + 1;//add one to the number of items in the heap

                                                    //Change whichList to show that the new item is on the open list.
                                                    whichList[a, b] = unVisited;

                                                    // Debug.Log("follower[" + followerId + "] whichList[" + a + "," + b + "] = " + (s = (whichList[a, b] == unVisited) ? "unVisited" : ""));

                                                }
                                                else
                                                {
                                                    ////8.If adjacent cell is already on the open list, check to see if this 
                                                    //	path to that cell from the starting location is a better one. 
                                                    //	If so, change the parent of the cell and its G and F costs.	
                                                    //Figure out the G cost of this possible new path
                                                    if (Mathf.Abs(a - parentX) == 1 && Math.Abs(b - parentY) == 1)
                                                        addedGCost = 14;//cost of going to diagonal tiles	
                                                    else
                                                        addedGCost = 10;//cost of going to non-diagonal tiles				
                                                    tempGcost = graphNodeInfo.mGCost[parentX, parentY] + addedGCost;
                                                    //If this path is shorter (G cost is lower) then change
                                                    //the parent cell, G cost and F cost. 	
                                                    if (tempGcost < graphNodeInfo.mGCost[a, b])
                                                    {
                                                        graphNodeInfo.MParentNode[a, b].x = parentX;
                                                        graphNodeInfo.MParentNode[a, b].y = parentY;
                                                        graphNodeInfo.mGCost[a, b] = tempGcost;
                                                        int[] openXYValue = null;
                                                        for (int x = 1; x <= graphNodeInfo.numberOfOpenListItems; x++)
                                                        {

                                                            openList.TryGetValue(x, out openXYValue);
                                                            if (openXYValue != null && openXYValue[0] == a && openXYValue[1] == b)
                                                            {
                                                                graphNodeInfo.mFCost[x] = graphNodeInfo.mGCost[a, b] + graphNodeInfo.mHCost[x];
                                                                m = x;
                                                                //See if changing the F score bubbles the item up from it's current location in the heap
                                                                while (m != 1)//While item hasn't bubbled to the top (m=1)	
                                                                {
                                                                    //Check if child is < parent. If so, swap them.	
                                                                    if (graphNodeInfo.mFCost[m] < graphNodeInfo.mFCost[m >> 1])
                                                                    {
                                                                        temp = graphNodeInfo.mFCost[m >> 1];
                                                                        graphNodeInfo.mFCost[m >> 1] = graphNodeInfo.mFCost[m];
                                                                        graphNodeInfo.mFCost[m] = temp;
                                                                        m /= 2;

                                                                    }
                                                                    else break;


                                                                }

                                                                break;
                                                            }
                                                        }

                                                    }

                                                }
                                            }

                                        }

                                    }
                                }

                            }

                        }
                        //if exist a target position x z in whichList,then found it
                        if (whichList[targetRow, targetCol] == unVisited)
                        {
                            UnityEngine.Debug.Log("follower[" + followerId + "] targetRow=" + targetRow + ",targetCol=" + targetCol + "have found");
                            pathInfo.STATE = PathInfo.PathState.ONE;
                            break;
                        }
                    }
                    else
                    {
                        pathInfo.STATE = PathInfo.PathState.NONE;
                        consumedTime = System.DateTime.Now - startTime;
                        UnityEngine.Debug.Log("follower[" + followerId + "] targetRow=" + targetRow + ",targetCol=" + targetCol + "have not found.consumed time:" + String.Format("{0}.{1}", consumedTime.Seconds, consumedTime.Milliseconds.ToString().PadLeft(3, '0')));
                        yield break;
                    }

                } while (!stopped);
                if (pathInfo.STATE == PathInfo.PathState.ONE)
                {
                    //a.Working backwards from the target to the starting location by checking
                    //	each cell's parent, figure out the length of the path.
                    consumedTime = System.DateTime.Now - startTime;
                    UnityEngine.Debug.Log("[FOLLOWER ID] :" + followerId + "I'm found target." + "[" + target.localPosition.ToString() + "] position,and consumed time:" + String.Format("{0}.{1}", consumedTime.Seconds, consumedTime.Milliseconds.ToString().PadLeft(3, '0')));
                    pathX = targetRow; pathY = targetCol;
                    if (!storedPathFinderPathLength.TryGetValue(followerId, out tempFollowerPathLength))
                        storedPathFinderPathLength.Add(followerId, 0);
                    //                    int pathLength = 0;
                    int cellPositionX = 0;
                    int cellPositionY = 0;
                    do
                    {
                        tempX = graphNodeInfo.MParentNode[pathX, pathY].x;
                        pathY = graphNodeInfo.MParentNode[pathX, pathY].y;
                        pathX = tempX;
                        //Figure out the path length
                        pathInfo.length = pathInfo.length + 1;
                    } while (pathX != startRow || pathY != startCol);
                    //b.Now copy the path information over to the databank. Since we are
                    //	working backwards from the target to the start location, we copy
                    //	the information to the data bank in reverse order. The result is
                    //	a properly ordered set of path data, from the first step to the
                    //	last.
                    pathX = targetRow; pathY = targetCol;
                    cellPositionX = pathInfo.length * 2;//start at the end	
                    pathInfo.pathBank.Clear();
                    do
                    {
                        cellPositionX = cellPositionX - 2;//work backwards 2 integers
                        // Debug.Log("------------------------->>>>>cellPositionX :pathX,pathY[" + pathX+","+pathY+"]  <<<<----------------------------");
                        pathInfo.pathBank.Add(new float[] { pathX, pathY });


                        //c.Look up the parent of the current cell.	
                        tempX = graphNodeInfo.MParentNode[pathX, pathY].x;
                        pathY = graphNodeInfo.MParentNode[pathX, pathY].y;
                        pathX = tempX;

                        //d.If we have reached the starting square, exit the loop.	
                    }

                    while (pathX != startRow || pathY != startCol);
                    //-----------------------DEBUG-----------------------------
                    //draw line on screen if enable debug 
                    if (false)
                    {
                        float[] curr = null;
                        float[] next = null;
                        for (int d = 0; d < pathInfo.pathBank.Count; d++)
                        {

                            curr = pathInfo.pathBank[d];
                            next = pathInfo.pathBank[d + 1];
                            UnityEngine.Debug.DrawLine(new Vector3(curr[0] + (mTileSize + mOffset) + (mTileSize / 2), 1.5f, curr[1] + (mTileSize + mOffset) + (mTileSize / 2)), new Vector3(next[0] + (mTileSize + mOffset) + (mTileSize / 2), 1.5f, next[1] + (mTileSize + mOffset) + (mTileSize / 2)), Color.green);

                        }

                    }
                    //e.mark this shorter path for follower

                    UnityEngine.Debug.Log("[FOLLOWER ID]:" + followerId + " starting to move. Total step is " + pathInfo.pathBank.Count);
                    ReadPath(followerId, startingX, startingY);

                }
            }



            yield return null;
        }




        private PathNode initializeRequiredParameters(int followerId, out PathInfo pathInfo)
        {
            PathNode pathNode = new PathNode();

            if (followersPathInfo.ContainsKey(followerId))
                followersPathInfo.Remove(followerId);
            if (followersGraphNodeInfo.ContainsKey(followerId))
                followersGraphNodeInfo.Remove(followerId);
            pathInfo = new PathInfo();
            followersPathInfo.Add(followerId, pathInfo);
            followersGraphNodeInfo.Add(followerId, pathNode);
            return pathNode;
        }

        public int step = 0;
        GameObject stepGameObject;
        public Vector3 ReadPath(int followerId, float startingX, float startingZ)
        {


            step++;
            GameObject go = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
            go.name = "follower_" + followerId + "(Clone)_" + step;
            go.transform.parent = stepGameObject.transform;
            go.transform.position = new Vector3(startingX, 1.49f, startingZ);
            go.transform.localScale = new Vector3(mTileSize / 3, .11f, mTileSize / 3);
            go.renderer.material.color = Color.clear;
            if (followersPathInfo[followerId].STATE == PathInfo.PathState.ONE)
            {
                // Debug.Log("step:=" + step);
                if (followersPathInfo[followerId].location < followersPathInfo[followerId].length)
                {
                    // if(followersPathInfo[followerId].location==0)
                    // {
                    followersPathInfo[followerId].location++;

                    //}

                    followersPathInfo[followerId].currPosition = ReadPathXZ(followerId, followersPathInfo[followerId].location);
                    if (followersPathInfo[followerId].location == followersPathInfo[followerId].length)
                    {
                        followersPathInfo[followerId].STATE = PathInfo.PathState.NONE;
                    }
                }

            }
            else
            {
                followersPathInfo[followerId].currPosition = Vector3.right * startingX + Vector3.up * mHeight + Vector3.forward * startingZ;
            }
            return followersPathInfo[followerId].currPosition;
        }
        public Vector3 ReadPathXZ(int followerId, int pathLocation)
        {
            Vector3 xyz = Vector3.zero;
            float[] xz = null;
            //Debug.Log("location:=" + followersPathInfo[followerId].location);
            // Debug.Log("followersPathInfo[followerId].pathBank.Count - 1 - pathLocation =" + (followersPathInfo[followerId].pathBank.Count - 1 - pathLocation));
            if (pathLocation <= followersPathInfo[followerId].length)
            {
                if (followersPathInfo[followerId].pathBank.Count - 1 - pathLocation >= 0)
                {

                    xz = followersPathInfo[followerId].pathBank[(followersPathInfo[followerId].pathBank.Count - 1 - pathLocation)];

                    xz[0] = xz[0] * (mTileSize + mOffset) + mTileSize / 2 + .45f;
                    xz[1] = xz[1] * (mTileSize + mOffset) + mTileSize / 2 + .45f;
                    xyz = new Vector3(xz[0], mHeight, xz[1]);
                    // Debug.Log("STEP:"+(step++)+"------------------------------>>>>>>>>>>>>>>>>>>xyz=[" + followersPathInfo[followerId].pathBank[(followersPathInfo[followerId].pathBank.Count - 1 - pathLocation)][0] + "," + mHeight + "," + followersPathInfo[followerId].pathBank[(followersPathInfo[followerId].pathBank.Count - 1 - pathLocation)][1] + "] <<<<<<<<<<<<<<<<<<<-------------------------------");
                }

            }
            return xyz;
        }
        public Vector3 ReadPath2(int followerId, float startingX, float startingZ, float startingY)
        {


            step++;
            GameObject go = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
            go.name = "follower_" + followerId + "(Clone)_" + step;
            go.transform.parent = stepGameObject.transform;
            go.transform.position = new Vector3(startingX, 1.49f, startingZ);
            go.transform.localScale = new Vector3(mTileSize / 3, .11f, mTileSize / 3);
            go.renderer.material.color = Color.clear;
            if (followersPath[followerId].STATE == PathInfo.PathState.ONE)
            {
                UnityEngine.Debug.Log("step:=" + step);
                if (followersPath[followerId].mPathInfo.location <= followersPath[followerId].mPathInfo.length)
                {
                    if (followersPath[followerId].mPathInfo.location != followersPath[followerId].mPathInfo.length)
                    {

                        followersPath[followerId].mPathInfo.location++;

                        followersPath[followerId].mPathInfo.currPosition = ReadPathXZ2(followerId, followersPath[followerId].mPathInfo.location, startingY);


                    }
                    else
                    {
                        step = 0;
                    }
                }

            }
            else
            {
                followersPath[followerId].mPathInfo.currPosition = new Vector3(startingX, startingY, startingZ);
            }


            return followersPath[followerId].mPathInfo.currPosition;
        }
        public Vector3 ReadPathXZ2(int followerId, int pathLocation, float startingY)
        {
            Vector3 xyz = Vector3.zero;
            float[] xz = null;
            int index = 0;
            //Debug.Log("location:=" + followersPathInfo[followerId].location);
            // Debug.Log("followersPathInfo[followerId].pathBank.Count - 1 - pathLocation =" + (followersPathInfo[followerId].pathBank.Count - 1 - pathLocation));
            if (pathLocation <= followersPath[followerId].mPathInfo.length)
            {
                index=followersPath[followerId].mPathInfo.pathBank.Count - pathLocation;
                if (index >= 0)
                {
                    xz = followersPath[followerId].mPathInfo.pathBank[(index)];

                    xz[0] = xz[0] * (mTileSize + mOffset) + mTileSize / 2 + .45f;
                    xz[1] = xz[1] * (mTileSize + mOffset) + mTileSize / 2 + .45f;
                    xyz = new Vector3(xz[0], startingY, xz[1]);
                }

            }
            return xyz;
        }

        #endregion

        public PathInfo.PathState searchingPath(CPath path)
        {
            CPath currPath = getPathData(path);
            UnityEngine.Debug.Log("MATHMATIC :----------->>>>[PATH START]:" + currPath.startingXY + ",[PATH TARGET]:" + currPath.targetXY);

            int[,] currWhichList =currPath.whichList;
            CBinaryHeap currBinaryHeap = adjustBinaryHeap(currPath.cBinaryHeap);
            resetPathInfo(currPath.mPathInfo);
            // PathInfo pathInfo = currPath.mPathInfo;
            UnityEngine.Debug.Log("[FOLLOWER ID]" + path.id + " calculating...");
            int startRow;
            int startCol;
            int targetRow;
            int targetCol;
            float startingX;
            float startingY;
            float destX;
            float destY;
            computingCoordinate(path.id, currPath.startingXY, currPath.targetXY, out startRow, out startCol, out targetRow, out targetCol, out startingX, out startingY, out destX, out destY);

            currBinaryHeap.clear();
            PathInfo.PathState pathState;
            if (preCalculatingPath(targetRow, targetCol, currPath, startingX, destX, startingY, destY, out pathState))
            {
                if (Math.Abs(startingX - destX) > 1 || Math.Abs(startingY - destY) > 1)
                {
                    //                    int u, v;
                    //                   u = v = 1;

                    currPath.node[startRow, startCol] = new GraphNode();
                    currPath.node[startRow, startCol].gcost = 0;
                    currPath.node[startRow, startCol].index = 1;
                    GraphNode.State.VISITED = GraphNode.State.VISITED + 2;
                    GraphNode.State.UNVISITED = GraphNode.State.VISITED - 1;
                    PathNode mHeapNodes = new PathNode();
                    mHeapNodes.x = startRow;
                    mHeapNodes.y = startCol;
                    currBinaryHeap.addNode(mHeapNodes);
                    currWhichList[startRow, startCol] = GraphNode.State.VISITED;//first,check the first node( ID#1 ) have already accessed.
                    startTime = System.DateTime.Now;

                    //openList.Add(1,1);
                    //	Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
                    PathInfo.PathState startPath;
                    if (aStarMethod(path.id, currBinaryHeap, currWhichList, currPath, targetRow, targetCol))
                        if (currPath.STATE == PathInfo.PathState.ONE)
                        {
                            if (calculatingRealPath(path.id, path.targetXY, targetRow, targetCol, currPath, startRow, startCol, startingX, startingY))
                            {
                                UnityEngine.Debug.Log("[FOLLOWER: " + currPath.id + " STATE :" + currPath.STATE + "]<<<<<<<<<<<<<<<<<<<<have done yet!>>>>>>>>>>>>>>>>>>>>>");
                                return currPath.STATE;

                            }
                        }
                        else
                        {
                            currPath.message = PathFindingMessage.ERROR.EXECUTE_PATH_NOT_FIND;
                            currPath.STATE = PathInfo.PathState.ERROR;
                            return currPath.STATE;
                        }
                    UnityEngine.Debug.Log("[[[[[[[[[[NEXT PATH]]]]]]]]]]]");
                }
            }
            return currPath.STATE;

        }

        /// <summary>
        /// Calulating real path track after a* algorithm method have computed a valid path.
        /// </summary>
        /// <param name="followerId"></param>
        /// <param name="target"></param>
        /// <param name="targetRow"></param>
        /// <param name="targetCol"></param>
        /// <param name="currPath"></param>
        /// <param name="startRow"></param>
        /// <param name="startCol"></param>
        /// <param name="startingX"></param>
        /// <param name="startingY"></param>
        private bool calculatingRealPath(int followerId, Vector3 target, int targetRow, int targetCol, CPath currPath,
            int startRow, int startCol, float startingX, float startingY)
        {
            int pathX;
            int pathY;
            int tempX;
            //a.Working backwards from the target to the starting location by checking
            //	each cell's parent, figure out the length of the path.
            consumedTime = System.DateTime.Now - startTime;
            UnityEngine.Debug.Log("[FOLLOWER ID] :" + followerId + "I'm found target." + "[" + target.ToString() +
                      "] position,and consumed time:" +
                      String.Format("{0}.{1}", consumedTime.Seconds, consumedTime.Milliseconds.ToString().PadLeft(3, '0')));
            // pathX = targetRow; pathY = targetCol;
            /* if (!storedPathFinderPathLength.TryGetValue(followerId, out tempFollowerPathLength))
                        storedPathFinderPathLength.Add(followerId, 0);
*/

            /*      do
                    {
                        tempX = currPath.node[pathX, pathY].parentNode.x;
                        pathY = currPath.node[pathX, pathY].parentNode.y;
                        pathX = tempX;
                        //Figure out the path length
                        currPath.mPathInfo.length = currPath.mPathInfo.length + 1;
                    } while (pathX != startRow || pathY != startCol);*/
            //b.Now copy the path information over to the databank. Since we are
            //	working backwards from the target to the start location, we copy
            //	the information to the data bank in reverse order. The result is
            //	a properly ordered set of path data, from the first step to the
            //	last.
            pathX = targetRow;
            pathY = targetCol;
            currPath.mPathInfo.pathBank.Clear();
            do
            {
                //cellPositionX = cellPositionX - 2;//work backwards 2 integers
                // Debug.Log("------------------------->>>>>cellPositionX :pathX,pathY[" + pathX+","+pathY+"]  <<<<----------------------------");
                currPath.mPathInfo.pathBank.Add(new float[] { pathX, pathY });


                //c.Look up the parent of the current cell.	
                tempX = currPath.node[pathX, pathY].parentNode.x;
                pathY = currPath.node[pathX, pathY].parentNode.y;
                pathX = tempX;
                currPath.mPathInfo.length = currPath.mPathInfo.length + 1;
                //d.If we have reached the starting square, exit the loop.	
            } while (pathX != startRow || pathY != startCol);
            //-----------------------DEBUG-----------------------------
            //draw line on screen if enable debug 
            if (false)
            {
                float[] curr = null;
                float[] next = null;
                for (int d = 0; d < currPath.mPathInfo.pathBank.Count - 1; d++)
                {
                    curr = currPath.mPathInfo.pathBank[d];
                    next = currPath.mPathInfo.pathBank[d + 1];
                    UnityEngine.Debug.DrawLine(
                        new Vector3(curr[0] + (mTileSize + mOffset) + (mTileSize / 2), 1.5f,
                            curr[1] + (mTileSize + mOffset) + (mTileSize / 2)),
                        new Vector3(next[0] + (mTileSize + mOffset) + (mTileSize / 2), 1.5f,
                            next[1] + (mTileSize + mOffset) + (mTileSize / 2)), Color.green);
                }
            }
            //e.mark this shorter path for follower
            UnityEngine.Debug.Log("[FOLLOWER ID]:" + followerId + " starting to move. Total step is " + currPath.mPathInfo.pathBank.Count);
            return true;
        }

        private bool aStarMethod(int followerId, CBinaryHeap currBinaryHeap, int[,] currWhichList, CPath currPath,
            int targetRow, int targetCol)
        {
            int a, b = 0;
            int parentX;
            int parentY;
            int v;
            int corner;
            PathNode pathNode;
            int tempGcost;
            currPath.STATE = PathInfo.PathState.NONE;
            do
            {
                //If the open list is not empty, take the first cell off of the list.
                //	This is the lowest F cost cell on the open list.
                if (currBinaryHeap.numberOfOpenList != 0)
                {
                    UnityEngine.Debug.Log("[CURRENT NUMBER:]" + currBinaryHeap.numberOfOpenList);
                    parentX = currBinaryHeap.mBinaryHeap[1].x;
                    parentY = currBinaryHeap.mBinaryHeap[1].y;
                    currWhichList[parentX, parentY] = GraphNode.State.VISITED;

                    // currBinaryHeap.numberOfOpenList = currBinaryHeap.numberOfOpenList - 1; ///reduce number of open list items by 1	
                    //	Delete the top item in binary heap and reorder the heap, with the lowest F cost item rising to the top.
                    //move the last item in the heap up to slot #1


                    v = 1;
                    //	Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
                    // Pop the first item off the open list.

                    #region Remove node in heap

                    currBinaryHeap.removeNode();

                    #endregion

                    int addedGCost = 0;
                    for (a = parentX - 1; a <= parentX + 1; a++)
                    {
                        for (b = parentY - 1; b <= parentY + 1; b++)
                        {
                            //1.we need a valid tile
                            if (a != -1 && b != -1 && a != mRows && b != mColumns)
                            {
                                //2.we need to put a unvisited position xy or a none-opened position into binary heap.
                                if (currWhichList[a, b] != GraphNode.State.VISITED)
                                {
                                    //	Don't cut across corners
                                    //3.if not obstacles
                                    if (mObstacleMap[a, b].walkable != GraphNode.State.UNWALKABLE)
                                    {
                                        //	Don't cut across corners
                                        corner = GraphNode.State.WALKABLE;
                                        if (a == parentX - 1)
                                        {
                                            if (b == parentY - 1)
                                            {
                                                if (mObstacleMap[parentX - 1, parentY].walkable == GraphNode.State.UNWALKABLE
                                                    || mObstacleMap[parentX, parentY - 1].walkable == GraphNode.State.UNWALKABLE)
                                                    corner = GraphNode.State.UNWALKABLE;
                                            }
                                            else if (b == parentY + 1)
                                            {
                                                if (mObstacleMap[parentX, parentY + 1].walkable == GraphNode.State.UNWALKABLE
                                                    || mObstacleMap[parentX - 1, parentY].walkable == GraphNode.State.UNWALKABLE)
                                                    corner = GraphNode.State.UNWALKABLE;
                                            }
                                        }
                                        else if (a == parentX + 1)
                                        {
                                            if (b == parentY - 1)
                                            {
                                                if (mObstacleMap[parentX, parentY - 1].walkable == GraphNode.State.UNWALKABLE
                                                    || mObstacleMap[parentX + 1, parentY].walkable == GraphNode.State.UNWALKABLE)
                                                    corner = GraphNode.State.UNWALKABLE;
                                            }
                                            else if (b == parentY + 1)
                                            {
                                                if (mObstacleMap[parentX + 1, parentY].walkable == GraphNode.State.UNWALKABLE
                                                    || mObstacleMap[parentX, parentY + 1].walkable == GraphNode.State.UNWALKABLE)
                                                    corner = GraphNode.State.UNWALKABLE;
                                            }
                                        }
                                        if (corner != GraphNode.State.UNWALKABLE)
                                        {
                                            //4.Create a new open list item in the binary heap.
                                            if (currWhichList[a, b] != GraphNode.State.UNVISITED)
                                            {
                                                pathNode = new PathNode();

                                                pathNode.x = a; //record coordinate x y for new item 
                                                pathNode.y = b;


                                                if (Mathf.Abs(a - parentX) == 1 && Mathf.Abs(b - parentY) == 1)
                                                {
                                                    addedGCost = 14;
                                                }
                                                else
                                                {
                                                    addedGCost = 10;
                                                }
                                                currPath.node[a, b] = new GraphNode();
                                                currPath.node[a, b].gcost = (currPath.node[parentX, parentY].gcost + addedGCost);
                                                currPath.node[a, b].parentNode.x = parentX;
                                                currPath.node[a, b].parentNode.y = parentY;
                                                pathNode.hCost = 10 * (Mathf.Abs(a - targetRow) + Mathf.Abs(b - targetCol));

                                                pathNode.fCost = pathNode.hCost + currPath.node[a, b].gcost;
                                                //2.Move the new open list item to the proper place in the binary heap.
                                                //Starting at the bottom, successively compare to parent items,
                                                //swapping as needed until the item finds its place in the heap
                                                //or bubbles all the way to the top (if it has the lowest F cost). 

                                                #region add node into heap.

                                                currBinaryHeap.addNode(pathNode);

                                                #endregion

                                                //Change whichList to show that the new item is on the open list.
                                                currWhichList[a, b] = GraphNode.State.UNVISITED;

                                                // Debug.Log("follower[" + followerId + "] whichList[" + a + "," + b + "] = " + (s = (whichList[a, b] == unVisited) ? "unVisited" : ""));
                                            }
                                        }
                                    }
                                }
                                else
                                {
                                    ////8.If adjacent cell is already on the open list, check to see if this 
                                    //	path to that cell from the starting location is a better one. 
                                    //	If so, change the parent of the cell and its G and F costs.	
                                    //Figure out the G cost of this possible new path
                                    if (Mathf.Abs(a - parentX) == 1 && Math.Abs(b - parentY) == 1)
                                        addedGCost = 14; //cost of going to diagonal tiles	
                                    else
                                        addedGCost = 10; //cost of going to non-diagonal tiles				
                                    tempGcost = currPath.node[parentX, parentY].gcost + addedGCost;
                                    //If this path is shorter (G cost is lower) then change
                                    //the parent cell, G cost and F cost. 	
                                    if (tempGcost < currPath.node[a, b].gcost)
                                    {
                                        currPath.node[a, b].parentNode.x = parentX;
                                        currPath.node[a, b].parentNode.y = parentY;
                                        currPath.node[a, b].gcost = tempGcost;

                                        for (int i = 1; i < currBinaryHeap.numberOfOpenList; i++)
                                        {
                                            if (currBinaryHeap.mBinaryHeap[i].x == a && currBinaryHeap.mBinaryHeap[i].y == b)
                                            {
                                                currBinaryHeap.mBinaryHeap[i].fCost = currPath.node[a, b].gcost +
                                                                                      currBinaryHeap.mBinaryHeap[i].hCost;
                                            }
                                        }

                                        #region reorder the heap,and lowest f cost on the top of this heap.

                                        currBinaryHeap.rearrange();

                                        #endregion
                                    }
                                }
                            }
                        }
                    }
                    if (a == targetRow && b == targetCol)
                    {
                        UnityEngine.Debug.Log("whether same: YES" +
                                  ((currWhichList[targetRow, targetCol] == GraphNode.State.UNVISITED) ? "true" : "false"));
                    }
                }
                else
                {
                    currPath.STATE = PathInfo.PathState.NONE;
                    consumedTime = System.DateTime.Now - startTime;
                    UnityEngine.Debug.Log("follower[" + followerId + "] targetRow=" + targetRow + ",targetCol=" + targetCol +
                              "have not found.consumed time:" +
                              String.Format("{0}.{1}", consumedTime.Seconds,
                                  consumedTime.Milliseconds.ToString().PadLeft(3, '0')));
                    {
                        break;
                    }
                }


                //if exist a target position x z in whichList,then found it
                if (currWhichList[targetRow, targetCol] == GraphNode.State.UNVISITED)
                {
                    UnityEngine.Debug.Log("follower[" + followerId + "] targetRow=" + targetRow + ",targetCol=" + targetCol + "have found");
                    currPath.STATE = PathInfo.PathState.ONE;
                    return true;
                }
            } while (!stopped);
            return false;
        }

        private bool preCalculatingPath(int targetRow, int targetCol, CPath currPath, float startingX, float destX,
            float startingY, float destY, out PathInfo.PathState pathState)
        {
            if (targetRow >= mRows || targetCol >= mColumns)
            {
                currPath.message = "could not reach it.";
                currPath.STATE = PathInfo.PathState.ERROR;
                {
                    pathState = currPath.STATE;
                    return false;
                }
            }
            //	If starting location and target are in the same location...
            if (Math.Abs(startingX - destX) < 1 && Math.Abs(startingY - destY) < 1)
            {
                currPath.message = "could not access";
                currPath.STATE = PathInfo.PathState.SAME;
                {
                    pathState = currPath.STATE;
                    return false;
                }
            }
            if (mObstacleMap[targetRow, targetCol].walkable == GraphNode.State.UNWALKABLE)
            {
                //TODO
                UnityEngine.Debug.Log("could not access.----CASE TWO");
                //notify to  target locaiton is not walkable. 
                currPath.STATE = PathInfo.PathState.UNWALKABLE;
                {
                    pathState = currPath.STATE;
                    return false;
                }
            }
            pathState = PathInfo.PathState.NONE;
            return true;
        }


    }
}